#pragma config(Sensor, in1,    RightLine,           sensorLineFollower)
#pragma config(Sensor, in2,    MidLine,             sensorLineFollower)
#pragma config(Sensor, in3,    LeftLine,            sensorLineFollower)
#pragma config(Sensor, dgtl1,  EncoderR,            sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  EncoderL,            sensorQuadEncoder)
#pragma config(Sensor, dgtl5,  Frontsonar,          sensorSONAR_inch)
#pragma config(Motor,  port1,           LeftLift1,     tmotorNormal, openLoop)
#pragma config(Motor,  port2,           DriveL,        tmotorNormal, openLoop)
#pragma config(Motor,  port3,           DriveL2,       tmotorNormal, openLoop)
#pragma config(Motor,  port4,           DriveR,        tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port5,           DriveR2,       tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port6,           Gate1,         tmotorNormal, openLoop)
#pragma config(Motor,  port7,           Gate2,         tmotorNormal, openLoop)
#pragma config(Motor,  port10,          LeftLift2,     tmotorNormal, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/* DRIVE FUNCTIONS
* drive_forward_1msec
* drive_straight
* drive_straight_to_touch
*/

/* drive_forward_msec
* @purpose: autonomously drive robot forward for provided duration
* @param speed: speed of motors, -127 to 127; negative values go reverse
* @param duration: duration in milliseconds
*/
int programselect = 1;
bool redteam = true;
bool programselecting = true;
const int totalprogramnumber = 5;
int count = 0;
const int threshold = 900;

void clearencoders(){
  SensorValue[EncoderL] = 0;
  SensorValue[EncoderR] = 0;

}



void Stop(){
  motor[DriveL] = 0;
  motor[DriveL2] = 0;
  motor[DriveR] = 0;
  motor[DriveR2] = 0;

}

void drive_forward_msec(int speed, int duration) {
  motor[DriveR] = speed;
  motor[DriveL] = speed;
  motor[DriveL2] = speed;
  motor[DriveR2] = speed;
  wait1Msec(duration);
  Stop();
}
/* drive_straight
* @purpose: autonomously drive robot forward for provided distance in inches
* precondition: 4 inch wheels used; if other wheels, change the 4 in the while loop
* @param speed: speed of motors, -127 to 127; negative values go reverse
* @param distance: distance in inches
* TODO: consider adding an acceleration loop
*///there are 360 ticks in an encoder revolution
void drive_straight(int speed, int distance) {
  clearencoders();
  while(SensorValue[EncoderL]< distance && SensorValue[EncoderR] < distance) {
    if(SensorValue[EncoderL] < SensorValue[EncoderR]) {
      motor[DriveR] = speed;
      motor[DriveL] = speed -1;
      motor[DriveL2] = speed -1;
      motor[DriveR2] = speed;
    }
    else if(SensorValue[EncoderL] < SensorValue[EncoderR]) {
      motor[DriveR] = speed -1;
      motor[DriveL] = speed;
      motor[DriveL2] = speed;
      motor[DriveR2] = speed -1;
    }
    else {
      motor[DriveR] = speed;
      motor[DriveL] = speed;
      motor[DriveL2] = speed;
      motor[DriveR2] = speed;
    }
  }
  Stop();

}





void straight(int speed, int distance) {
  clearencoders();
  count = (abs(SensorValue[EncoderR]) + abs(SensorValue[EncoderL]))/2;
  while(count < distance) {
    if(SensorValue[EncoderL] > SensorValue[EncoderR]) {
      motor[DriveR] = -speed;
      motor[DriveL] = -speed -1;
      motor[DriveL2] = -speed -1;
      motor[DriveR2] = -speed;
    }
    else if(SensorValue[EncoderL] < SensorValue[EncoderR]) {
      motor[DriveR] = -speed -1;
      motor[DriveL] = -speed;
      motor[DriveL2] =-speed;
      motor[DriveR2] = -speed -1;
    }
    else {
      motor[DriveR] = -speed;
      motor[DriveL] = -speed;
      motor[DriveL2] = -speed;
      motor[DriveR2] = -speed;
    }
  }
  Stop();

}

void TurnEnc(char power, int ticks){
  clearencoders();


  do{
    motor[DriveL] = power;
    motor[DriveL2] = power;
    motor[DriveR] = -power;
    motor[DriveR2] = -power;

    count = (abs(SensorValue[EncoderR]) + abs(SensorValue[EncoderL]))/2;

  }while(count <= ticks);

  Stop();
}


/* drive_straight_unlocked
* @purpose: autonomously drive robot forward for provided distance in inches WHILE allowing
* precondition: 4 inch wheels used; if other wheels, change the 4 in the while loop
* @param speed: speed of motors, -127 to 127; negative values go reverse
* @param distance: distance in inches
* TODO: consider adding an acceleration loop
*///there are 360 ticks in an encoder revolution
void drive_straight_unlocked(int speed, float distance) {
  clearencoders();
  if( ( (float) abs(SensorValue[EncoderL])  * 2 / 360 * PI) < distance &&
    ( (float) abs(SensorValue[EncoderR]) * 2 / 360 * PI) < distance) {
    if(abs(SensorValue[EncoderL]) > abs(SensorValue[EncoderR])) {
      motor[DriveR] = -speed;
      motor[DriveL]  = -speed + 1;
    }
    else if(abs(SensorValue[EncoderL]) < abs(SensorValue[EncoderR])) {
      motor[DriveR] = -speed + 1;
      motor[DriveL]  = -speed;
    }
    else {
      motor[DriveR] = -speed;
      motor[DriveL]  = -speed;
    }
  }
}

/* drive_straight_to_touch
* @purpose: autonomously drive robot forward until touch sensors are triggered
* precondition: 2 touch sensors, left and right
* @param speed: speed of motors, -127 to 127; negative values go reverse
*/
/*void drive_straight_to_touch(int speed) {
while(SensorValue[LeftTouch]  == 0 || SensorValue[RightTouch] == 0) {
if(SensorValue[EncoderL] > SensorValue[EncoderR]) {
motor[DriveR] = speed;
motor[DriveL]  = speed - 1;
}
else if(SensorValue[EncoderL] < SensorValue[EncoderR]) {
motor[DriveR] = speed - 1;
motor[DriveL]  = speed;
}
else {
motor[DriveR] = speed;
motor[DriveL]  = speed;
}
}
motor[DriveL] = 0;
motor[DriveR]= 0;
}*/



/* turn
* @purpose: autonomously execute a point turn
* @param speed: speed of motors, 0 to 127; no negative values
* @param degrees: number of degrees to turn
* TODO: complete this function
*/
/*void turner(int speed, int degrees) {
SensorValue[EncoderL] = 0;
SensorValue[EncoderR] = 0;
while(SensorValue[EncoderL] < degrees || SensorValue[EncoderR] < degrees*2) {
motor[DriveL] = speed;
motor[DriveR] = -speed;
}
motor[DriveL] = 0;
motor[DriveR] = 0;
}*/

/*void gate(int speed) {
motor[Gate1] = speed;
motor[Gate2] = speed;
wait1Msec(2000);
motor[Gate1] = 0;
motor[Gate2] = 0;
}*/

/*void lift(int speed) {
motor[LeftLift1] = speed;
motor[LeftLift2] = speed;
wait1Msec(2000);
motor[LeftLift1] = 0;
motor[LeftLift2] = 0;
}*/

/*void turn_msec(int speed, int duration) {

if(){
motor[DriveL]=-speed;
motor[DriveR]=speed;
wait1Msec(duration);
motor[DriveL]=0;
motor[DriveR]=0;}
else{
motor[DriveL]=speed;
motor[DriveR]=-speed;
wait1Msec(duration);
motor[DriveL]=0;
motor[DriveR]=0;}
}*/

/* MODE SELECTION FUNCTION
* mode_select
* @purpose use a potentiometer to set mode
* high potentiometer values return 1, low potentiometer values return 0
*/

/*int mode_select() {
if(SensorValue[Mode] >= 2000)
return 1;
else
return 0;
}*/


/* LINE FOLLOWER FUNCTIONS
* follow_line_msec
* follow_line_distance
*/

/*follow_line_msec
* @purpose follow a line for provided duration using 3 light sensors
* @param speed: speed of motors, 0 to 127
* @param duration: duration of line following in milliseconds
*/
/*void follow_line_msec(int speed, int duration) {
ClearTimer(T1);
int threshold = 250;  //guessed threshold; low numbers are lighter
while(time1[T1] < duration) {
if(SensorValue[MidLine] < threshold) {  //middle sensor is on line
motor[DriveL]  = speed;
motor[DriveR] = speed;
}
else if(SensorValue[LeftLine] < threshold) {  //left sensor is on line
motor[DriveL]  = speed - 1;
motor[DriveR] = speed;
}
else if(SensorValue[RightLine] < threshold) { //right sensor is on line
motor[DriveL]  = speed;
motor[DriveR] = speed - 1;
}
}
}*/


/*follow_line_distance
* @purpose follow a line for provided duration using 3 light sensors
* precondition: 4-inch wheels
* @param speed: speed of motors, 0 to 127
* @param distance: distance to travel in inches
*/
/*void follow_line_distance(int speed, int ticks, int threshold) {
//guessed threshold; low numbers are lighter
//while(SensorValue[EncoderL] && SensorValue[EncoderR]<ticks){
while(true){
if(SensorValue[MidLine] < threshold) {  //middle sensor is on line
motor[DriveR] = speed;
motor[DriveL] = speed;
motor[DriveL2] = speed;
motor[DriveR2] = speed;
}
else if(SensorValue[LeftLine] < threshold) {  //left sensor is on line
motor[DriveR] = speed;
motor[DriveL] = speed-1;
motor[DriveL2] = speed-1;
motor[DriveR2] = speed;
}
else if(SensorValue[RightLine] < threshold) { //right sensor is on line
motor[DriveR] = speed-1;
motor[DriveL] = speed;
motor[DriveL2] = speed;
motor[DriveR2] = speed-1;
}
}

}*/
void waitstop(time){
 motor[DriveL] = 0;
  motor[DriveL2] = 0;
  motor[DriveR] = 0;
  motor[DriveR2] = 0;
wait1Msec(time);

}
/*void linefollow(){
  ClearTimer(T4)
  while(true);
  {
    // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -\
    displayLCDCenteredString(0, "LEFT  CNTR  RGHT");        //  Display   |
    displayLCDPos(1,0);                                     //  Sensor    |
    displayNextLCDNumber(SensorValue(LeftLine));    //  Readings  |
    displayLCDPos(1,6);                                     //  to LCD.   |
    displayNextLCDNumber(SensorValue(MidLine));  //            |
    displayLCDPos(1,12);                                    //  L  C  R   |
    displayNextLCDNumber(SensorValue(RightLine);   //  x  x  x   |
    // - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -/

    // RIGHT sensor sees dark:
    if(SensorValue(RightLine) < threshold)
    {
      // counter-steer right:
      motor[DriveL]  = 0;
      motor[DriveL2] = 0;
      motor[DriveR] = 63;
      motor[DriveR2] = 63;
    }
    // CENTER sensor sees dark:
    if(SensorValue(MidLine) < threshold)
    {
      // go straight
      motor[DriveL]  = 63;
      motor[DriveL2] = 63;
      motor[DriveR] = 63;
      motor[DriveR2] = 63;
    }
    // LEFT sensor sees dark:
    if(SensorValue(LeftLine)< threshold)
    {
      // counter-steer left:
      motor[DriveL]  = 63;
      motor[DriveL2] = 63;
      motor[DriveR] = 0;
      motor[DriveR2] = 0;
    }
  }
}



*/

void Rampage (int speed){

  if(SensorValue[EncoderL] > SensorValue[EncoderR]) {
    motor[DriveR] = speed;
    motor[DriveL] = speed -1;
    motor[DriveL2] = speed -1;
    motor[DriveR2] = speed;
  }
  else if(SensorValue[EncoderL] < SensorValue[EncoderR]) {
    motor[DriveR] = speed -1;
    motor[DriveL] = speed;
    motor[DriveL2] = speed;
    motor[DriveR2] = speed -1;
  }
  else {
    motor[DriveR] = speed;
    motor[DriveL] = speed;
    motor[DriveL2] = speed;
    motor[DriveR2] = speed;

  }
}

void foward_until()
{

  while(SensorValue[RightLine] >= 2000){
    Rampage(63);
  }


}

void Line_straight(int speed, float Target){
  clearencoders();
 while(SensorValue[EncoderL] <= Target){
 if(SensorValue[MidLine] < threshold) {  //middle sensor is on line
          Rampage(63);
      }
      else if(SensorValue[LeftLine] < threshold) {  //left sensor is on line
          motor[DriveR] = speed-80;
        motor[DriveL] = speed;
        motor[DriveL2] = speed;
        motor[DriveR2] = speed-80;
      }
      else if(SensorValue[RightLine] < threshold) { //right sensor is on line
          motor[DriveR] = speed;
        motor[DriveL] = speed-80;
        motor[DriveL2] = speed-80;
        motor[DriveR2] = speed;

      }




    }

  }

  void linesonar(int power, int inches, float precaution){
 ClearTimer(T4);
  clearencoders();
 while(SensorValue[Frontsonar] <= inches || time1[T4] < precaution * 1000){//if the sonar sees somethin or if the timer is close to x number of seconds ..,
 if(SensorValue[MidLine] < threshold) {  //middle sensor is on line
          Rampage(63);
      }
      else if(SensorValue[LeftLine] < threshold) {  //left sensor is on line
         motor[DriveR] = power;
        motor[DriveL] = power-80;
        motor[DriveL2] = power-80;
        motor[DriveR2] = power;
      }
      else if(SensorValue[RightLine] < threshold) { //right sensor is on line
          motor[DriveR] = power;
        motor[DriveL] = power;
        motor[DriveL2] = power-80;
        motor[DriveR2] = power-80;

      }




    }
    Stop();
  }

//function:

void screenrefresh()
{
  clearLCDLine(1);
  displayLCDPos(1,0);
  if (redteam == true)
  {
    displayNextLCDString("RED ");
  }
  if (redteam == false)
  {
    displayNextLCDString("BlUE");
  }
  displayNextLCDString("    ");
  displayNextLCDNumber(programselect);
  displayNextLCDString("     OK");
}
